
#ifndef PX_SPHERE_H
#define PX_SPHERE_H

#include <pxVector3.h>

namespace Px {
namespace Geometry {

template <typename Real>
class Sphere {
    using Vector3 = Math::Vector3<Real>;

public:
    Sphere();
    Sphere(const Sphere& sphere);
    Sphere(const Vector3& center, Real radius);
    virtual ~Sphere();

    void set(const Vector3& center, Real radius);
    void set(Real cx, Real cy, Real cz, Real radius);
    void setCenter(const Vector3& center);
    void setRadius(Real radius);

    bool contains(const Vector3& point);
    bool intersectPlane(const Vector3& normal, const Vector3& point);
    bool intersectSphere(const Sphere<Real>& sphere);
    bool intersectSphere(const Vector3& otherCenter, Real otherRadius);

    double volume(double pi = 3.14159) const;
    double diameter() const;

    static bool ContainsPoint(const Vector3& center, Real radius, const Vector3& point);
    static bool ContainsPoint(const Sphere<Real>& sphere, const Vector3& point);
    static bool IntersectPlane(const Sphere<Real>& sphere, const Vector3& normal, const Vector3& point);
    static bool IntersectSphere(const Sphere<Real>& a, const Sphere<Real>& b);
    static bool InstersectSphere(const Sphere<Real>& sphere, const Vector3& otherCenter, Real otherRadius);

    double getVolume(double pi = 3.14159) const;
    double getDiameter() const;
    Vector3 getCenter() const;
    Real getRadius() const;

    Sphere<Real>& operator = (const Sphere<Real>& sphere);

protected:
    Vector3 center;
    Real radius;
};

template <typename Real>
Sphere<Real>::Sphere() {
    this->center = Vector3::Zero();
    this->radius = static_cast<Real>(1);
}

template <typename Real>
Sphere<Real>::Sphere(const Sphere& sphere) {
    this->center = sphere.center;
    this->radius = sphere.radius;
}

template <typename Real>
Sphere<Real>::Sphere(const Vector3& center, Real radius) {
    this->center = center;
    this->radius = radius;
}

template <typename Real>
Sphere<Real>::~Sphere() {}

template <typename Real>
void Sphere<Real>::set(const Vector3& center, Real radius) {
    this->center = center;
    this->radius = radius;
}

template <typename Real>
void Sphere<Real>::set(Real cx, Real cy, Real cz, Real radius) {
    this->center = Vector3(cx, cy, cz);
    this->radius = radius;
}

template <typename Real>
void Sphere<Real>::setCenter(const Vector3& center) {
    this->center = center;
}

template <typename Real>
void Sphere<Real>::setRadius(Real radius) {
    this->radius = radius;
}

template <typename Real>
bool Sphere<Real>::contains(const Vector3& point) {
    if ( Vector3::Distance(this->center, point) < this->radius ) return true;
    return false;
}

template <typename Real>
bool Sphere<Real>::intersectPlane(const Vector3& normal, const Vector3& point) {
    return Sphere<Real>::IntersectPlane(*this, normal, point);
}

template <typename Real>
bool Sphere<Real>::intersectSphere(const Sphere<Real>& sphere) {
    if ( Vector3::Distance(this->center, sphere.center) <= this->radius + sphere.radius ) return true;
    return false;
}

template <typename Real>
double Sphere<Real>::volume(double pi) const {
    return (4.0 / 3.0) * pi * std::pow(this->radius, 3.0);
}

template <typename Real>
double Sphere<Real>::diameter() const {
    return this->radius * static_cast<Real>(2);
}

template <typename Real>
bool Sphere<Real>::ContainsPoint(const Vector3& center, Real radius, const Vector3& point) {
    if ( Vector3::Distance(center, point) <= radius ) return true;
    return false;
}

template <typename Real>
bool Sphere<Real>::ContainsPoint(const Sphere<Real>& sphere, const Vector3& point) {
    if ( Vector3::Distance(sphere.center, point ) <= sphere.radius ) return true;
    return false;
}

template <typename Real>
bool Sphere<Real>::IntersectPlane(const Sphere<Real>& sphere, const Vector3& normal, const Vector3& point) {
    if ( sphere.radius == static_cast<Real>(0) ) return false;
    Vector3 normalizedNormal = normal.normalized();

    //--------------------------------------------------------------------------
    // For a normalized plane (|p.n| = 1), evaluating the plane equation
    // for a point gives the signed distance of the point to the plane
    //--------------------------------------------------------------------------
    Real d = -point.dot(normalizedNormal);
    Real dist = Vector3::Dot(sphere.center, normalizedNormal) - d;

    //--------------------------------------------------------------------------
    // If sphere center with +/- radius from plane, plane intersects sphere
    //--------------------------------------------------------------------------
    return std::abs(dist) <= sphere.radius;
}

template <typename Real>
bool Sphere<Real>::IntersectSphere(const Sphere<Real>& a, const Sphere<Real>& b) {
    if ( Vector3::Distance(a.center, b.center) <= a.radius + b.radius ) return true;
    return false;
}

template <typename Real>
bool Sphere<Real>::InstersectSphere(const Sphere& sphere, const Vector3& otherCenter, Real otherRadius) {
    if ( Vector3::Distance(sphere.center, otherCenter) <= sphere.radius + otherRadius ) return true;
    return false;
}

template <typename Real>
double Sphere<Real>::getVolume(double pi) const {
    return return (4.0 / 3.0) * pi * std::pow(this->radius, 3.0);
}

template <typename Real>
double Sphere<Real>::getDiameter() const {
    return this->radius * static_cast<Real>(2);
}

template <typename Real>
typename Sphere<Real>::Vector3 Sphere<Real>::getCenter() const {
    return this->center;
}

template <typename Real>
Real Sphere<Real>::getRadius() const {
    return this->radius;
}

template <typename Real>
Sphere<Real>& Sphere<Real>::operator = (const Sphere<Real>& sphere) {
    if ( this == &sphere ) return *this;

    this->center = sphere.center;
    this->radius = sphere.radius;
    return *this;
}

typedef Sphere<float> Spheref;
typedef Sphere<double> Sphered;

}

}

#endif
